/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: JointGroupCtrlServer.cpp,v 1.15 2009-05-13 07:18:17 amaula Exp $
 *
 * \file
 * \brief Implementation of JointGroupCtrl interface Server.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */

#include "JointGroupCtrlServer.hpp"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::JointGroupCtrl;
//*****************************************************************************
//*****************************************************************************

CJointGroupCtrlServer::CJointGroupCtrlServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, 
					     const int aInterfaceMinor)
  : CMaCI(aMaCICtrlServerPtr, GIMI_PROTOCOL_MACI_JOINTGROUPCTRL, aInterfaceMinor),
    CThread(1),
    CSync(1,1),
    iServerActive(false),
    iJointGroupCtrlCommandArray(),
    iReplyDataArray(),
    iJointInformationArray(),
    iJointInformationSet(false),
    iEventTypes(),
    cmdCount(0)
{
}
//*****************************************************************************
     
CJointGroupCtrlServer::~CJointGroupCtrlServer()
{
  DoClose();
}
//*****************************************************************************

MaCI::EMaCIError CJointGroupCtrlServer::DoOpen(void)
{
  cmdCount =0;
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);
  // Execute only if server is not active.
  if (!iServerActive&& iJointInformationSet) {
    RegisterProvidedService("MaCI - JointGroupCtrl Interface");
    RegisterAcceptedService("MaCI - JointGroupCtrl CommandInterface");
  
    // Start the handler Thread
    iServerActive = true;
    SetActive(true);
    CThread::RunThread();

    result = KMaCIOK;
    
    dPrint(4,"Succesfully Open():ed JointGroupCtrlServer instance %p.", this);
  }else{
    if(iServerActive){
      dPrint(3,"Server already opened");
    }
    if(!iJointInformationSet){
      dPrint(3,"JointInformation has not been set!");
    }
  }

  return result;
}
//*****************************************************************************

MaCI::EMaCIError CJointGroupCtrlServer::DoClose(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);

  // Execute block only if server is currently active.
  if (iServerActive) {
    // Unregister service
    UnregisterProvidedService();
    
    // Stop the handler Thread
    iServerActive = false;
    SetActive(false);
    /* CThread::CancelThread(); //Removed by script */
    CThread::WaitThread();
    
    
    dPrint(4,"Succesfully Close():ed JointGroupCtrlServer instance %p", this);
  }
  
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CJointGroupCtrlServer::WaitCommand(CJointGroupCtrlData &aData, 
						    TReplyInfo &aInfo,
						    const unsigned int aTimeout_ms,
						    bool aSkipQueued)
{
  EMaCIError result = KMaCIError;
  const unsigned int begin = ownTime_get_ms();
  int timeout = aTimeout_ms;
  bool dataSaved = false;
  aData.Reset();

  Lock();
  do {
    const unsigned int asize = iJointGroupCtrlCommandArray.size();
    // Check whether there are commands in buffer.
    if (asize > 0) {
      // There are commands
      
      const CJointGroupCtrlData *d;
      const TReplyInfo *r;
      dataSaved = true;

      // Check whether we want to skip queue 
      // SKIPPING SAME MESSAGES
      if (aSkipQueued && asize > 1) {
        dPrint(100,"THere are %d messages in queue",asize);
         //First get the oldest command
        //Then check if there are newer same commands with the same jointnumber
        d = iJointGroupCtrlCommandArray[0];
        r = iReplyDataArray[0];

        //Save the command type and check for joint number(if command != GetJointGroupStatus)
        uint32_t cmd = d->GetCommand()->cmd;
        std::vector<unsigned int> jointIndexes;
        EJointGroupCtrlType dataType = JointGroupCtrl::KTypeUnknown;

        if(cmd != KCommandGetJointGroupStatus && cmd !=KCommandUpdateState ){
          if(d->GetJointSpeedPtrArray().size() > 0){
            for(std::vector<unsigned int>::size_type i = 0; i< d->GetJointSpeedPtrArray().size();i++){
              jointIndexes.push_back(d->GetJointSpeedPtrArray().at(i)->jointNumber);

            }
            dataType = KTypeJointSpeed;
          }else if(d->GetJointPositionPtrArray().size() > 0){
            for(std::vector<unsigned int>::size_type i = 0; i< d->GetJointPositionPtrArray().size();i++){
              jointIndexes.push_back( d->GetJointPositionPtrArray().at(i)->jointNumber);
              
            }
            dataType = KTypeJointPosition;
          }else if(d->GetJointForcePtrArray().size() > 0){
            for(std::vector<unsigned int>::size_type i = 0; i< d->GetJointForcePtrArray().size();i++){
              jointIndexes.push_back( d->GetJointForcePtrArray().at(i)->jointNumber);

            }
            dataType = KTypeJointForce;
          }
        }

        //Check for newer commands of the same type and same joint number. Do not even try to understand ;)
        std::vector<const TReplyInfo *>::iterator replyIterator= iReplyDataArray.begin()+1; 
        for(std::vector<const CJointGroupCtrlData *>::iterator i= iJointGroupCtrlCommandArray.begin()+1; 
            i != iJointGroupCtrlCommandArray.end();
            i++){

          //Same command
          if((*i)->GetCommand()->cmd == cmd){

            if(cmd == KCommandGetJointGroupStatus || cmd == KCommandUpdateState){
              d = *i;
              r = *replyIterator;
            }else{
              if(dataType == KTypeJointSpeed && (*i)->GetJointSpeedPtrArray().size() == jointIndexes.size()){
                bool sameIndexes = true;
               
                for(unsigned int savedJointIndex = 0; savedJointIndex <jointIndexes.size(); savedJointIndex++){
                  bool thisIndexFound = false;
                  for(unsigned int j = 0; j <(*i)->GetJointSpeedPtrArray().size(); j++){
                    if((*i)->GetJointSpeedPtrArray().at(j)->jointNumber == jointIndexes[savedJointIndex]){
                      thisIndexFound = true;
                    }
                  }
                  if(!thisIndexFound){
                    sameIndexes = false;
                  }
                }               

                if(sameIndexes){
                  dPrint(10,"Found absolutely same kind of command...");
                  d = *i;
                  r = *replyIterator;
                }

              }else if(dataType == KTypeJointPosition && (*i)->GetJointPositionPtrArray().size() >= jointIndexes.size()){
                bool sameIndexes = true;
                dPrint(10,"Same type of command (Position)");
                for(unsigned int savedJointIndex = 0; savedJointIndex <jointIndexes.size(); savedJointIndex++){
                  bool thisIndexFound = false;
                  for(unsigned int j = 0; j <(*i)->GetJointPositionPtrArray().size(); j++){
                    dPrint(10,"Commands joint number %d, wanted %d",(*i)->GetJointPositionPtrArray().at(j)->jointNumber,jointIndexes[savedJointIndex]);
                    if((*i)->GetJointPositionPtrArray().at(j)->jointNumber == jointIndexes[savedJointIndex]){
                      thisIndexFound = true;
                    }
                  }
                  if(!thisIndexFound){
                    sameIndexes = false;
                  }
                }               

                if(sameIndexes){
                  dPrint(10,"Found absolutely the same kind of command...");
                  d = *i;
                  r = *replyIterator;
                }
              }else if(dataType == KTypeJointForce&& (*i)->GetJointForcePtrArray().size() >= jointIndexes.size()){
                bool sameIndexes = true;
               
                for(unsigned int savedJointIndex = 0; savedJointIndex <jointIndexes.size(); savedJointIndex++){
                  bool thisIndexFound = false;
                  for(unsigned int j = 0; j <(*i)->GetJointForcePtrArray().size(); j++){
                    if((*i)->GetJointForcePtrArray().at(j)->jointNumber == jointIndexes[savedJointIndex]){
                      thisIndexFound = true;
                    }
                  }
                  if(!thisIndexFound){
                    sameIndexes = false;
                  }
                }               

                if(sameIndexes){
                  dPrint(10,"Found absolutely same kind of command...");
                  d = *i;
                  r = *replyIterator;
                }


              }else{
                dPrint(109,"Skipped");
              }
            }
          }
          

          replyIterator++;
        }
        // Then, copy data of the newest command
        aData = *d;
	aInfo = *r;

        //After all this, remove and delete all this type of commands with same joint index found in the arrays
        replyIterator = iReplyDataArray.begin();
        for(std::vector<const CJointGroupCtrlData *>::iterator i= iJointGroupCtrlCommandArray.begin(); 
            i != iJointGroupCtrlCommandArray.end();){

          //Same command
          bool deleted = false;
          if((*i)->GetCommand()->cmd == cmd){
            if(cmd == KCommandGetJointGroupStatus || cmd == KCommandUpdateState){
              delete *i;
              delete *replyIterator;
              i = iJointGroupCtrlCommandArray.erase(i);
              replyIterator = iReplyDataArray.erase(replyIterator);
              //SPEED
            }else if(dataType == KTypeJointSpeed){
              if((*i)->GetJointSpeedPtrArray().size() == jointIndexes.size()){
                bool sameIndexes = true;
               
                for(unsigned int savedJointIndex = 0; savedJointIndex <jointIndexes.size(); savedJointIndex++){
                  bool thisIndexFound = false;
                  for(unsigned int j = 0; j <(*i)->GetJointSpeedPtrArray().size(); j++){
                    if((*i)->GetJointSpeedPtrArray().at(j)->jointNumber == jointIndexes[savedJointIndex]){
                      thisIndexFound = true;
                    }
                  }
                  if(!thisIndexFound){
                    sameIndexes = false;
                  }
                }               

                if(sameIndexes){
                  delete *i;
                  delete *replyIterator;
                  i= iJointGroupCtrlCommandArray.erase(i);
                  replyIterator = iReplyDataArray.erase(replyIterator);
                  deleted = true;
                }

              }
              //POSITION
            }else if(dataType == KTypeJointPosition){
              if((*i)->GetJointPositionPtrArray().size() == jointIndexes.size()){
                bool sameIndexes = true;
               
                for(unsigned int savedJointIndex = 0; savedJointIndex <jointIndexes.size(); savedJointIndex++){
                  bool thisIndexFound = false;
                  for(unsigned int j = 0; j <(*i)->GetJointPositionPtrArray().size(); j++){
                    if((*i)->GetJointPositionPtrArray().at(j)->jointNumber == jointIndexes[savedJointIndex]){
                      thisIndexFound = true;
                    }
                  }
                  if(!thisIndexFound){
                    sameIndexes = false;
                  }
                }               

                if(sameIndexes){
                 
                  delete *i;
                  delete *replyIterator;
                  i= iJointGroupCtrlCommandArray.erase(i);
                  replyIterator = iReplyDataArray.erase(replyIterator);
                  deleted = true;
                }

              }
              //FORCE
            }else if(dataType == KTypeJointForce){
              if((*i)->GetJointForcePtrArray().size() == jointIndexes.size()){
                bool sameIndexes = true;
               
                for(unsigned int savedJointIndex = 0; savedJointIndex <jointIndexes.size(); savedJointIndex++){
                  bool thisIndexFound = false;
                  for(unsigned int j = 0; j <(*i)->GetJointForcePtrArray().size(); j++){
                    if((*i)->GetJointForcePtrArray().at(j)->jointNumber == jointIndexes[savedJointIndex]){
                      thisIndexFound = true;
                    }
                  }
                  if(!thisIndexFound){
                    sameIndexes = false;
                  }
                }               

                if(sameIndexes){
                  delete *i;
                  delete *replyIterator;
                  i= iJointGroupCtrlCommandArray.erase(i);
                  replyIterator = iReplyDataArray.erase(replyIterator);
                  deleted = true;
                }

              }    
              
            }

            if(!deleted){
              i++;
              replyIterator++;
            }
            
              

            
          }
        }
        //NO SKIPPING
      } else {
        // No skipping, so get front() reference;
        d = iJointGroupCtrlCommandArray.front();
	r = iReplyDataArray.front();
        // Copy data.
        aData = *d;
	aInfo = *r;
        // Delete source
        delete d;
	delete r;
        // pop the entry from queue.
        iJointGroupCtrlCommandArray.erase(iJointGroupCtrlCommandArray.begin());
	iReplyDataArray.erase(iReplyDataArray.begin());
        

      }

      // Set result.
      result = KMaCIOK;
      
      // Break the loop, skipping Wait()
      break;
    }
    
    // Wait signal unconditionally.
    if (timeout > 0) Wait(timeout);
    
    // Recalculate timeout
    timeout = aTimeout_ms - ownTime_get_ms_since(begin);
  } while(timeout > 0 && iServerActive);
  Unlock();

  // Change result type if timeout was the reason.
  if (timeout <= 0 && !dataSaved) result = KMaCITimeout;


  return result;
  
}
//*****************************************************************************

MaCI::EMaCIError CJointGroupCtrlServer::SendReply(CJointGroupCtrlData &aData,
						  TReplyInfo &aInfo)
{
  EMaCIError result = KMaCIError;
  if(iServerActive){
    gim::binbag::CBinBag replybb;
    dPrint(10,"Sending reply");
    aData.Print(10);
    
    int r = iGIMIPtr->send(aInfo.senderId,
			   aData.GetBinBagContainerPtr()->GetBuffer(),
			   aData.GetBinBagContainerPtr()->GetBufferSize(),
			   iInterfaceMajor,
			   iInterfaceMinor,
			   aInfo.msgId);
    dPrint(8,"major %d, minor %d, senderid %d, msgid %d",
	   iInterfaceMajor,
	   iInterfaceMinor,
	   aInfo.senderId,
	   aInfo.msgId);
    
    if (r == GIMI_OK) {
      dPrint(8,"Succesfully sent message with %d BinBag elements to '%p'",
	     aData.GetBinBagContainerPtr()->GetElementCount(),
	     aInfo.senderId);

      result = KMaCIOK;
      
    } else {
      
      dPrint(5,"Failed to send message with %d BinBag elements to '%p'",
	     aData.GetBinBagContainerPtr()->GetElementCount(),
	     aInfo.senderId);
    }
  }else{
    result = KMaCINoConnection;
    
  }

  return result;

}
//*****************************************************************************

int CJointGroupCtrlServer::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(4, "GIMIMessage handler started");
  
  // Run as long as the thread should be active
  while(iServerActive) {
    // Call the ReceiveMessage wrapper with 1s timeout
    e = ReceiveInterfaceMessage(gmsg, 1000);
    if (e == KMaCIOK) {
      cmdCount++;      
      dPrint(8,"Got GIMIMessage %d - Processing. %d.Command", gmsg.messageId,cmdCount);

      ProcessMessage(gmsg);
      
    } else if (e != KMaCITimeout) {
      // Other error, try to recover by sleeping one second.
      ownSleep_ms(1000);
      
    }
  }
  
  dPrint(4, "GIMIMessage handler stopped");
  
  return result;

}
//*****************************************************************************

int CJointGroupCtrlServer::ProcessMessage(const gimi::GIMIMessage &msg)
{
 int result = 0;
  gim::binbag::CBinBag bb;
  CJointGroupCtrlData d;
  bool r;
 
  r = bb.Decode(msg.getData(), msg.getDatalength());
  if (r) {
    // Succesfully decoded as BinBag.
    r = d.DecodeFrom(&bb);
    if (r) {
      // Succesfully decoded as SpeedCtrl
      // Now, Check whether it was Command, if so, process it 
      gim::binbag::CBinBag replybb;
      CJointGroupCtrlData reply;
      bool send_reply = false;
      // Do encoding to this binbag
      reply.EncodeTo(&replybb);
      
      const TCommand *cmd = d.GetCommand();
      if (cmd) {

        switch(cmd->cmd) {
          
          // Set Commands: by default passed to user
	case KCommandGetSpeed:
	case KCommandGetPosition:
	case KCommandGetForce:
        case KCommandSetGroupSpeeds:
        case KCommandSetGroupPositions:
        case KCommandSetGroupForces:
        case KCommandGetJointGroupStatus:
        case KCommandGetGroupPositions:
        case KCommandGetGroupSpeeds:
        case KCommandGetGroupForces:
        case KCommandUpdateState:

	  {
          // Default action for Set commands is to save them to CommandQueue
          
          // Debug
          dPrint(8, "Got Command to be passed to User (Command wass %d)",
                 cmd->cmd);
          
          // Create new dynamic entry

	  CJointGroupCtrlData *new_entry = new CJointGroupCtrlData(d);
	  TReplyInfo *new_reply = new TReplyInfo(msg.getSenderId(),msg.messageId);
          // Lock, Push, Notify, Unlock
          Lock();
          iJointGroupCtrlCommandArray.push_back(new_entry);
	  iReplyDataArray.push_back(new_reply);
          Notify();
          Unlock();
          break;
        }
	
	case KCommandGetJointInformation:{
	  dPrint(1,"asked for JOINT information");
	  unsigned int size = iJointInformationArray.size();
	  reply.SetCommand(TCommand(KReplyGetJointInformation));
	  reply.SetJointInformationHeader(TJointInformationHeader(size));
	  for(unsigned int i = 0; i < size; ++i){
	    reply.SetJointInformation(iJointInformationArray[i]);
	  }
	  reply.Print(6);
	  send_reply = true;
	  break;
	}
        case  KCommandGetEventTypes:{
          dPrint(6,"Asked for event types.");
          reply.SetCommand(TCommand(KReplyGetEventTypes));
          reply.SetEventTypes(iEventTypes);
          send_reply = true;
          break;

          break;
        }

        default:
          dPrint(1,"Unidentified command! %d (%x)! Ignore.",
                 cmd->cmd, cmd->cmd);
          break;
        }
      }
      
      // Check whether reply needs to be sent
      if (send_reply) {
	dPrint(1,"Sending Reply");
	TReplyInfo rInfo(msg.getSenderId(),msg.messageId);
	dPrint(8,"Replyinfo %d, %d", rInfo.senderId,rInfo.msgId);
	SendReply(reply,rInfo);
      }
    }
  }
  
  return result;
}
//*****************************************************************************
void CJointGroupCtrlServer::ClearCommandBuffer(void)
{
  Lock();
  dPrint(8,"Clearing commandbuffer, got %d entries", iJointGroupCtrlCommandArray.size());
  iJointGroupCtrlCommandArray.clear();
  iReplyDataArray.clear();
  Unlock();
}
//*****************************************************************************

void CJointGroupCtrlServer::SetJointGroupInfo(TJointInformationArray &aArray)
{
  Lock();
  iJointInformationArray = aArray;
  iJointInformationSet = true;
  Unlock();
}
//*****************************************************************************

bool CJointGroupCtrlServer::SetEventTypes(TEventTypes &aType)
{
  iEventTypes = aType;
  //dPrint(1,"eventtype setted to %d",iEventTypes.continuousTypes);
  return false;
}
//*****************************************************************************

bool CJointGroupCtrlServer::SendEvent(const CJointGroupCtrlData &aData)
{
  bool result = false;

  assert(iGIMIPtr != NULL);
  const TCommand *com = aData.GetCommand();
  if(com){
    if(com->cmd == KCommandEvent){
   
      EMaCIError e = 
        SendInterfaceSubscriberMessage(aData.GetBinBagContainerPtr()->GetBuffer(),
                                       aData.GetBinBagContainerPtr()->GetBufferSize());
      result = (e == KMaCIOK);
    }else{
      dPrint(4,"Wrong command set, should be KCommandEvent");
    }

  } else {
    dPrint(4,"No Command set in container, will not send!");
 
  }

  return result;

}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************


